/* The is the Rover Team Project Code for The California Institute of Technology 2012.
Copyright (C) 2012 scchang, bryanhe, dflicker

This program is free software; you can redistribute it and/or
modify it under the terms of the GNU General Public License
as published by the Free Software Foundation; either version 2
of the License, or (at your option) any later version.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program; if not, write to the Free Software
Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA. */
#ifndef ROVER_STATE_
#define ROVER_STATE_

#include "include/rover_state.h"

RoverState::RoverState(void){
  bzero(comps, NUM_ROVER_COMS);
}

unsigned char RoverState::getValue(RoverState_Component comp){
  
  // Sanity Check
  int intValue = (int)comp;
  if(intValue < 0 || intValue >= NUM_ROVER_COMS){
    fprintf(stderr, "In RoverState::getValue, invalid RoverState_Component.\n");
    return (unsigned char)128;
  }

  return this->comps[intValue];
}

int RoverState::setValue(RoverState_Component comp, unsigned char value){

  // Sanity Check
  int intValue = (int)comp;
  if(intValue < 0 || intValue > NUM_ROVER_COMS){
    fprintf(stderr, "In RoverState::setValue, invalid InputState_Component.\n");
    return -1;
  }

  this->comps[intValue] = value;
  return 0;
}

int RoverState::toString(unsigned char** buffer){
  // We add two for the inputs
  int size = (2 * NUM_ROVER_COMS) + 1;
  unsigned char* output = new unsigned char[size];

  // Buffer Zero
  bzero(output, size);

  // Endpoints
  output[0] = STRING_STAR_POINT;
  output[size - 1] = STRING_END_POINT;

  // Add Delinators
  for(int i = 2; i < size - 1; i += 2){
    output[i] = STRING_DELINATOR;
  }

  // Now Actual Information
  int index = 0;
  for(int i = 1; i < size - 1; i += 2, index++){
    output[i] = this->comps[index];
  }

  *buffer = output;

  return 0;
}

RoverState RoverState::getRoverState(unsigned char* input){
  int size = (2 * NUM_ROVER_COMS) + 1;
  
  // Sanity Check
  if(check_correct_delinate(input, size) < 0){
    fprintf(stderr, "In RoverState::getRoverState, Incorrect Delination of Input String.\n");
    return RoverState();
  }

  // Assuming correct delination
  int index = 0;
  RoverState output = RoverState();
  for(int i = 1; i < size - 1; i += 2, index++){
    output.comps[index] = input[i];
  }

  return output;
}

void RoverState::verify(void){
  RoverState input = RoverState();
  unsigned char assign[NUM_ROVER_COMS];
  unsigned char* string_form;

  for(int i = 0; i < NUM_ROVER_COMS; i++){
    assign[i] = (unsigned char)(i);
    input.setValue((RoverState_Component)i, assign[i]);
  }

  for(int i = 0; i < NUM_ROVER_COMS; i++){
    assert(input.getValue((RoverState_Component)i) == assign[i]);
  }

  input.toString(&string_form);

  RoverState state = getRoverState(string_form);

  for(int i = 0; i < NUM_ROVER_COMS; i++){
    assert(state.getValue((RoverState_Component)i) == input.getValue((RoverState_Component)i));
  }

  delete []string_form;
}

#endif
